#include "scene_base.h" 

void SceneBase::ShowScene() {
    // 开始绘制，easyX的用法，将这一桢数据全部绘制完成在进行显示
    BeginBatchDraw();
    // 清屏
    cleardevice();
    // 显示道路，道路父指针指向子类函数进行调用
    road0->ShowRoad();
    // 显示车辆，父指针指向子类方法
    car0->ShowCar(BLACK);
    //绘制轨迹线
    if (SHOWCIRCLE && car0->p_center) {
        car0->ShowCircle();
    }
    // 结束绘制
    EndBatchDraw();
    //延时。由于Dealy方法是公共接口，可以直接调用
    Delay(DELAYTIME);
}

// 直行行驶到指定的距离
void SceneBase::UniformStraight(const double& total_s) {
    // 更新车辆直行状态的信息
    car0->UpdateStraightInfo();
    double s = 0.0;
    while (s < total_s) {
        s += fabs(car0->speed_y);
        car0->MoveStraightStep();
        ObsMoveStep();
        ShowScene();
    }
    car0->CoutInfo();
}

// 直行行驶到指定的速度
void SceneBase::UniformAccBySpeed(const double& target_speed_y) {
    // 车速大于0进入循环,自车以帧间进行行驶
    while (car0->pmid->y > 0.0) {
        car0->MoveStraightStep();
        ObsMoveStep();
        // 如果相差的速度大继续加速
        if (fabs(car0->speed_y - target_speed_y) > fabs(car0->a_y)) {
            car0->speed_y += car0->a_y;
        } else {
            // 否则,认为已经到达target_y,让加速度为0
            car0->speed_y = target_speed_y;
            car0->a_y = 0.0;
            if (target_speed_y == 0) {
                // 如果目标是停车,需要跳出循环,不然car0移动不到0.0的位置???????
                break;
            }
        }
        ShowScene();
    }
    car0->CoutInfo();
}

// 直行行驶指定距离时到达指定速度
void SceneBase::UniformAccByDis(const double& dis, const double& target_y) {
    // 计算加速度
    car0->a_y = (std::pow(car0->speed_y, 2) - std::pow(target_y, 2)) / 2.0 / dis;
    std::cout << "a_y = " << car0->a_y << ", dis = " << dis << std::endl;
    UniformAccBySpeed(target_y);
}

// 直行，行驶指定的时间到达指定的速度
void SceneBase::UniformAccByTime(const double& time, const double& target_speed_y) {
    // 计算加速度, time单位为s
    double freq = time * 1000 / DELAYTIME; // 计算循环了多少次,就是走了多少单位时间总步长
    car0->a_y = (target_speed_y - car0->speed_y) / freq;
    std::cout << "a_y = " << car0->a_y << ", time = " << time << std::endl;
    UniformAccBySpeed(target_speed_y);
}

/*******************转向****************/
void SceneBase::CarTurn(const int& turn_state, const double& R, const double& total_theta) {
    // 更新转向信息
    car0->UpdateTurnInfo(turn_state, R);
    double theta = 0.0;
    while (theta < total_theta) {
        theta += fabs(car0->delta_theta);               
        // 修正角度误差
        CorrectAngleError(car0->delta_theta, theta - total_theta);
        car0->CarTurnStep();
        ObsMoveStep();
        ShowScene();
    }
    car0->CoutInfo();
}

void SceneBase::LaneChange(const Point& target_point, const int& type, const double& s) {
    double dis = car0->pmidr->DistanceTo(target_point);
    Vec2d vec0(dis, car0->heading_theta + PI / 2.0);
    Vec2d vec(*car0->pmidr, target_point);
    double L = vec0.CrossProd(vec) / dis / 2.0;
    double H = vec0.InnerProd(vec) / dis / 2.0;  
    if (fabs(L) < 1e-10) {
        UniformStraight(dis);
        return;
    }
    // 计算转向半径R
    double R = (std::pow(L, 2) + std::pow(H, 2)) / (2.0 * fabs(L));
    // 计算目标转向角
    double target_theta = asin(H / R);
    // 计算转向角速度
    double  target_delta_theta = fabs(car0->speed / R);
    
    if (L > 1e-6) {
        // 向左绕行,对于单移线来讲方向需要先向左在向右,双移线类似
        // 1. 往左走
        car0->delta_theta = target_delta_theta;
        CarTurn(TurnDirection::TurnLeft, R, target_theta);

        if (type == singleType) {
            std::cout << "=====================" << std::endl;
            // 2. 向右打方向
            car0->delta_theta = -target_delta_theta;
            CarTurn(TurnDirection::TurnRight, R, target_theta);
        } else if (type == doubleType) {
            // 2. 向右打方向
            std::cout << "++++++++++++++++++++++" << std::endl;
            car0->delta_theta = -target_delta_theta;
            CarTurn(TurnDirection::TurnRight, R, target_theta);
            std::cout << "-----------straight s: " << s << std::endl; 
            // 3. 直行s的距离
            UniformStraight(s);
            // 4. 向右打方向
            car0->delta_theta = -target_delta_theta;
            CarTurn(TurnDirection::TurnRight, R, target_theta);
            // 5.向左打方向
            car0->delta_theta = target_delta_theta;
            CarTurn(TurnDirection::TurnLeft, R, target_theta);
        }
    } else if (L < -1e-6) {
        // 向右绕行
        car0->delta_theta = -target_delta_theta;
        CarTurn(TurnDirection::TurnRight, R, target_theta);
        if (type == singleType) {
            car0->delta_theta = target_delta_theta;
            CarTurn(TurnDirection::TurnLeft, R, target_theta);
        } else {
            car0->delta_theta = target_delta_theta;
            CarTurn(TurnDirection::TurnLeft, R, target_theta);

            UniformStraight(s);

            car0->delta_theta = target_delta_theta;
            CarTurn(TurnDirection::TurnLeft, R, target_theta);

            car0->delta_theta = -target_delta_theta;
            CarTurn(TurnDirection::TurnRight, R, target_theta);
        }
    }
    
}